#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>

#include "platform.h"
#include "drivers/time.h"
#include "drivers/nvic.h"
#include "build/debug.h"

#include "flight/mixer.h"
#include "drivers/io.h"
#include "pwm_output.h"
#include "timer.h"
#include "drivers/pwm_output.h"

static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmOutputPort_t servos[1];

static bool pwmMotorsEnabled = false;

static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{

    TIM_OCInitTypeDef TIM_OCInitStructure;

    TIM_OCStructInit(&TIM_OCInitStructure);
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;

    if (output & TIMER_OUTPUT_N_CHANNEL) {
        TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
        TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
        TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
    } else {
        TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
        TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
        TIM_OCInitStructure.TIM_OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
    }
    TIM_OCInitStructure.TIM_Pulse = value;

    timerOCInit(tim, channel, &TIM_OCInitStructure);
    timerOCPreloadConfig(tim, channel, TIM_OCPreload_Enable);

}

//const uint32_t hz = clock / prescaler;// 72000000 / 3 24000000
//const unsigned period = hz / pwmRateHz;// 24000000 / 480 = 50000
//idlePulse = 0
static void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardwareVal, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
{

    configTimeBase(timerHardwareVal->tim, period, hz);
    pwmOCConfig(timerHardwareVal->tim,
        timerHardwareVal->channel,
        value,
        inversion ? timerHardwareVal->output ^ TIMER_OUTPUT_INVERTED : timerHardwareVal->output
        );

    TIM_CtrlPWMOutputs(timerHardwareVal->tim, ENABLE);
    TIM_Cmd(timerHardwareVal->tim, ENABLE);

    channel->ccr = timerChCCR(timerHardwareVal);

    channel->tim = timerHardwareVal->tim;

    *channel->ccr = 0;
}

void pwmWriteMotor(uint8_t index, float value)
{
#ifdef I_AM_REMOTE_CONTROLLER
    if(index==0){
        *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
    }
#else
    /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
    //ccr is uint16_t;
#ifdef IS_BOAT
    if(index==0){
        int val = lrintf(((value - 1000) / 1000) * 2001);//5000
        *motors[index].channel.ccr = val;
    }else{
        int val = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
        *motors[index].channel.ccr = val;
    }
#else
    *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
#endif

#endif
}

void pwmWriteServo(uint8_t index, float value)
{
    *servos[index].channel.ccr = lrintf((value * servos[index].pulseScale) + servos[index].pulseOffset);
}

void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
{
    for (int index = 0; index < motorCount; index++) {
        // Set the compare register to 0, which stops the output pulsing if the timer overflows
        if (motors[index].channel.ccr) {
            *motors[index].channel.ccr = 0;
        }
    }
}

void pwmEnableMotors(void)
{
    /* check motors can be enabled */
    pwmMotorsEnabled = true;//(pwmWrite != &pwmWriteUnused);
}

bool pwmAreMotorsEnabled(void)
{
    return pwmMotorsEnabled;
}

#ifdef I_AM_REMOTE_CONTROLLER
static int flg = 0;
#define BEEP_H beepGpio->BSRR = beepPin
#define BEEP_L beepGpio->BRR = beepPin
static uint16_t beepPin;
static GPIO_TypeDef* beepGpio;
bool beepFlg = false;

void TIM2_IRQHandler(){
    if(TIM2->SR & TIM_FLAG_Update){
        TIM2->SR = (uint16_t)~TIM_FLAG_Update;
        if(beepFlg){
            if(flg){
                flg = 0;
                BEEP_H;
            }else{
                flg = 1;
                BEEP_L;
            }
        }
    }
    if(TIM2->SR & TIM_FLAG_CC1){
        TIM2->SR = (uint16_t)~TIM_FLAG_CC1;
    }
}
#endif

void proc(int motorIndex, pwmOutputPort_t *dev, const timerHardware_t *timerHardwareVal, uint16_t idlePulse);

void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{
    UNUSED(idlePulse);
    UNUSED(motorConfig);
#ifdef I_AM_REMOTE_CONTROLLER
    IO_t beep = IOGetByTag(IO_TAG(BEEP_PIN));
    beepPin = IO_Pin(beep);
    beepGpio = IO_GPIO(beep);
    IOConfigGPIO(beep, IO_CONFIG(GPIO_Mode_Out_PP,      GPIO_Speed_50MHz));
    UNUSED(motorConfig);
    UNUSED(motorCount);
    UNUSED(pwmOutConfig);
    

    {//IRQ
        NVIC_InitTypeDef NVIC_InitStructure;
        NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_BUILD_PRIORITY(1, 1));
        NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_BUILD_PRIORITY(1, 1));
        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
        NVIC_Init(&NVIC_InitStructure);
    }

    //TIM1->CR1=0;
    TIM2->CR1 |= TIM_CR1_CKD_1;
    TIM2->PSC = 1000;//0xFFFF; //72000000 / 65536
    TIM2->ARR = 20000; // 顶
    TIM2->DIER |= TIM_DIER_UIE | TIM_DIER_CC1IE;
    TIM2->CR1 |= TIM_CR1_CEN;//TIM_CR1_CKD_1 TIM_CR1_OPM | 

    // TIM2->CNT = 0;
    TIM2->CCR1 = 1;// 中间
    
    TIM2->SR, TIM2->DIER;
#endif
    memset(motors, 0, sizeof(motors));
    memset(servos, 0, sizeof(servos));


    int maxMmotor;
#ifdef I_AM_REMOTE_CONTROLLER
    maxMmotor = 1;
#else
    maxMmotor = MAX_SUPPORTED_MOTORS;
#endif



    for (int motorIndex = 0; motorIndex < maxMmotor && motorIndex < motorCount; motorIndex++) {
        const timerHardware_t *timerHardwareVal = &timerHardwareMotor[motorIndex];

        proc(motorIndex, &motors[motorIndex], timerHardwareVal, 0);

    }

#ifndef I_AM_REMOTE_CONTROLLER
    for (int servoIndex = 0; servoIndex < 1; servoIndex++) {
        const timerHardware_t *timerHardwareVal = &timerHardwareServo[servoIndex];

        proc(servoIndex, &servos[servoIndex], timerHardwareVal, 0);
    }
#endif

    pwmMotorsEnabled = true;

}

void proc(int motorIndex, pwmOutputPort_t *dev, const timerHardware_t *timerHardwareVal, uint16_t idlePulse){
    
    int pwmRateHz = BRUSHLESS_MOTORS_PWM_RATE;//motorConfig->motorPwmRate;//BRUSHLESS_MOTORS_PWM_RATE 200
#ifdef IS_BOAT
    // if(motorIndex==0||motorIndex==1){
    //     pwmRateHz = 5000;
    //     debuginfoSlowInt[40] = 123;
    // }
#endif
    dev->io = IOGetByTag(timerHardwareVal->tag);
    
    IOConfigGPIO(dev->io, IOCFG_AF_PP);

    /* standard PWM outputs */
    // margin of safety is 4 periods when unsynced
    //const unsigned pwmRateHz = 100;//100 is ok?


    const uint32_t clock = timerClock(timerHardwareVal->tim);//return SystemCoreClock; = SYSCLK_FREQ_72MHz  72000000
    /* used to find the desired timer frequency for max resolution */
    unsigned int prescaler = (clock / pwmRateHz); // 72000000/200 = 150000
    prescaler = (prescaler + 0xffff) / 0x10000; // 150000 + 0xffff = 215535 / 65536 = 3

    //debuginfoSlowInt[8*motorIndex + 0] = pwmRateHz;
    //debuginfoSlowInt[8*motorIndex + 1] = prescaler;

    const uint32_t hz = clock / prescaler;// 72000000 / 3 24000000
    unsigned period = hz / pwmRateHz;// 24000000 / 480 = 50000

    //debuginfoSlowInt[8*motorIndex + 2] = hz;
    //debuginfoSlowInt[8*motorIndex + 3] = period;
    /*
        if brushed then it is the entire length of the period.
        TODO: this can be moved back to periodMin and periodLen
        once mixer outputs a 0..1 float value.
    */
    float sMin;
    bool brushed = false;
    bool motorPwmInversion = false;
    if(brushed){
        dev->pulseScale = (period);
        sMin = 0;
    }else{
        float sLen = 1e-3f;//0.001
        dev->pulseScale = (sLen * hz) / 1000.0f;
        sMin = 1e-3f;//0.001
    }
    dev->pulseOffset = (sMin * hz) - (dev->pulseScale * 1000);
#ifdef IS_BOAT
    if(motorIndex==0){
        period = 2000;
    }
#endif
    //debuginfoSlowInt[8*motorIndex + 4] = idlePulse;
    //debuginfoSlowInt[8*motorIndex + 5] = motorPwmInversion;
    pwmOutConfig(&dev->channel, timerHardwareVal, hz, period, idlePulse, motorPwmInversion);

    dev->enabled = true;
}

pwmOutputPort_t *pwmGetMotors(void)
{
    return motors;
}
